% 
% x = [x2;...;x_{H-1};y2;...;y2;y_{2H-1};Δt;t'] α
% 改变(x0,y0),(xf,yf),H,a,b,c,d可以求不同情况下的最优路径
% 自己根据文献写的irm法程序，找到作者写的irma.m函数后发现几乎一样

clear
close all

%initial condition
x0 =  0; y0 =  0;
xf = 10; yf = 10;
H = 20;  %要求H>=6
V = 1;
u_max = 0.2;
w = 1.5;
delta = 1e-4;
a = [5];  %a,b,c,d为椭圆障碍物参数，椭圆的方程为(xi-cj)^2/aj^2-(yi-dj)^2/bj^2=1
b = [5];
c = [5];
d = [5];
m = size(a,2);  %椭圆障碍物个数
l_x = [-2*ones(H-2,1);  %x的下界
    0*ones(H-2,1);      %y的下界
    0;                  %Δt的下界
    0];                 %t'的下界
u_x = [10.2*ones(H-2,1);%x的上界
    12*ones(H-2,1);     %y的上界
    20/(H-1);           %Δt的上界
    (20/(H-1))^2];      %t'的上界

time_per_iter = [];

%define objective matrix
Q0 = zeros(2*H-1,2*H-1);  %minimize( (H-1)*Δt )
Q0(2*H-1,2*H-3) = (H-1)/2;
Q0(2*H-3,2*H-1) = (H-1)/2;

%define equality constraints
Eqcon(1).Q = zeros(2*H-1,2*H-1); %α^2=1
Eqcon(1).Q(2*H-1,2*H-1) = 1;
Eqcon(1).c = -1;

Eqcon(2).Q = zeros(2*H-1,2*H-1); %t'=(Δt)^2
Eqcon(2).Q(2*H-3,2*H-3) = -1;
Eqcon(2).Q(2*H-1,2*H-2) = 0.5;
Eqcon(2).Q(2*H-2,2*H-1) = 0.5;
Eqcon(2).c = 0;

Eqcon(3).Q = zeros(2*H-1,2*H-1); %x2^2+y2^2- V^2*(Δt)^2=0
Eqcon(3).Q(1,1) = 1;
Eqcon(3).Q(H-1,H-1) = 1;
Eqcon(3).Q(2*H-1,1) = -x0;
Eqcon(3).Q(1,2*H-1) = -x0;
Eqcon(3).Q(2*H-1,H-1) = -y0;
Eqcon(3).Q(H-1,2*H-1) = -y0;
Eqcon(3).Q(2*H-3,2*H-3) = -V^2;
Eqcon(3).c = x0^2+y0^2;

%(x_{h+1}-x_h)^2 + (y_{h+1}-y_h)^2 - V^2*(Δt)^2 = 0, h=2,...,H-2
for i = 1:H-3
    Eqcon(3+i).Q = zeros(2*H-1,2*H-1);
    Eqcon(3+i).Q(i,i) = 1;
    Eqcon(3+i).Q(i,i+1) = -1;
    Eqcon(3+i).Q(i+1,i) = -1;
    Eqcon(3+i).Q(i+1,i+1) = 1;
    Eqcon(3+i).Q(H-2+i,H-2+i) = 1;
    Eqcon(3+i).Q(H-2+i,H-2+i+1) = -1;
    Eqcon(3+i).Q(H-2+i+1,H-2+i) = -1;
    Eqcon(3+i).Q(H-2+i+1,H-2+i+1) = 1;
    Eqcon(3+i).Q(2*H-3,2*H-3) = -V^2;
    Eqcon(3+i).c = 0;
end

%x_{H-1}^2 + y_{H-1}^2 - 20*x_{H-1} - 20*y_{H-1} - V^2*(Δt)^2=0
Eqcon(H+1).Q = zeros(2*H-1,2*H-1);
Eqcon(H+1).Q(H-2,H-2) = 1;
Eqcon(H+1).Q(2*H-4,2*H-4) = 1;
Eqcon(H+1).Q(2*H-1,H-2) = -xf;
Eqcon(H+1).Q(H-2,2*H-1) = -xf;
Eqcon(H+1).Q(2*H-1,2*H-4) = -yf;
Eqcon(H+1).Q(2*H-4,2*H-1) = -yf;
Eqcon(H+1).Q(2*H-3,2*H-3) = -V^2;
Eqcon(H+1).c = xf^2+yf^2;

%define inequality constraints
%l_x<=x<=u_x
for i = 1:2*H-2  %x2~x_{H-1}的取值范围
    Incon(i).Q = zeros(2*H-1,2*H-1); %x(i)-u_x(i)<=0
    Incon(i).Q(2*H-1,i) = 0.5;
    Incon(i).Q(i,2*H-1) = 0.5;
    Incon(i).c = -u_x(i);
end
for i = 1:2*H-2  %x2~x_{H-1}的取值范围
    Incon(2*H-2+i).Q = zeros(2*H-1,2*H-1); %-x(i)+l_x(i)<=0
    Incon(2*H-2+i).Q(2*H-1,i) = -0.5;
    Incon(2*H-2+i).Q(i,2*H-1) = -0.5;
    Incon(2*H-2+i).c = l_x(i);
end

%x3^2+4*x2^2 - 4*x2*x3 + x3^2+4*x2^2 - 4*x2*x3 - V^2*u_max^2*(t')^2 = 0
Incon(4*H-3).Q = zeros(2*H-1,2*H-1); 
Incon(4*H-3).Q(1,1) = 4;
Incon(4*H-3).Q(1,2) = -2;
Incon(4*H-3).Q(2,1) = -2;
Incon(4*H-3).Q(2,2) = 1;
Incon(4*H-3).Q(H-1,H-1) = 4;
Incon(4*H-3).Q(H-1,H) = -2;
Incon(4*H-3).Q(H,H-1) = -2;
Incon(4*H-3).Q(H,H) = 1;
Incon(4*H-3).Q(2*H-1,1) = -2*x0;
Incon(4*H-3).Q(1,2*H-1) = -2*x0;
Incon(4*H-3).Q(2*H-1,2) = x0;
Incon(4*H-3).Q(2,2*H-1) = x0;
Incon(4*H-3).Q(2*H-1,H-1) = -2*y0;
Incon(4*H-3).Q(H-1,2*H-1) = -2*y0;
Incon(4*H-3).Q(2*H-1,H) = y0;
Incon(4*H-3).Q(H,2*H-1) = y0;
Incon(4*H-3).Q(1,1) = 4;

Incon(4*H-3).Q(2*H-2,2*H-2) = -V^2*u_max^2;
Incon(4*H-3).c = x0^2+y0^2;

for i = 1:H-4
    Incon(4*H-3+i).Q = zeros(2*H-1,2*H-1);
    Incon(4*H-3+i).Q(i,i) = 1;
    Incon(4*H-3+i).Q(i,i+1) = -2;
    Incon(4*H-3+i).Q(i,i+2) = 1;
    Incon(4*H-3+i).Q(i+1,i) = -2;
    Incon(4*H-3+i).Q(i+1,i+1) = 4;
    Incon(4*H-3+i).Q(i+1,i+2) = -2;
    Incon(4*H-3+i).Q(i+2,i) = 1;
    Incon(4*H-3+i).Q(i+2,i+1) = -2;
    Incon(4*H-3+i).Q(i+2,i+2) = 1;
    Incon(4*H-3+i).Q(H-2+i,H-2+i) = 1;
    Incon(4*H-3+i).Q(H-2+i,H-2+i+1) = -2;
    Incon(4*H-3+i).Q(H-2+i,H-2+i+2) = 1;
    Incon(4*H-3+i).Q(H-2+i+1,H-2+i) = -2;
    Incon(4*H-3+i).Q(H-2+i+1,H-2+i+1) = 4;
    Incon(4*H-3+i).Q(H-2+i+1,H-2+i+2) = -2;
    Incon(4*H-3+i).Q(H-2+i+2,H-2+i) = 1;
    Incon(4*H-3+i).Q(H-2+i+2,H-2+i+1) = -2;
    Incon(4*H-3+i).Q(H-2+i+2,H-2+i+2) = 1;
    Incon(4*H-3+i).Q(2*H-2,2*H-2) = -V^2*u_max^2;
    Incon(4*H-3+i).c = 0;
end

%x_{H-2}^2+4*x_{H-1}^2+20*x_{H-2}-40*x_{H-1}-4*x_{H-2}*x_{H-1}+
%y_{H-2}^2+4*y_{H-1}^2+20*y_{H-2}-40*y_{H-1}-4*y_{H-2}*y_{H-1}
%-V^2*u_max^2*(t')^2+200 = 0
Incon(5*H-6).Q = zeros(2*H-1,2*H-1); 
Incon(5*H-6).Q(H-3,H-3) = 1;
Incon(5*H-6).Q(H-3,H-2) = -2;
Incon(5*H-6).Q(H-2,H-3) = -2;
Incon(5*H-6).Q(H-2,H-2) = 4;
Incon(5*H-6).Q(2*H-1,H-3) = xf;
Incon(5*H-6).Q(H-3,2*H-1) = xf;
Incon(5*H-6).Q(2*H-1,H-2) = -2*xf;
Incon(5*H-6).Q(H-2,2*H-1) = -2*xf;
Incon(5*H-6).Q(2*H-5,2*H-5) = 1;
Incon(5*H-6).Q(2*H-5,2*H-4) = -2;
Incon(5*H-6).Q(2*H-4,2*H-5) = -2;
Incon(5*H-6).Q(2*H-4,2*H-4) = 4;
Incon(5*H-6).Q(2*H-1,2*H-5) = yf;
Incon(5*H-6).Q(2*H-5,2*H-1) = yf;
Incon(5*H-6).Q(2*H-1,2*H-4) = -2*yf;
Incon(5*H-6).Q(2*H-4,2*H-1) = -2*yf;
Incon(5*H-6).Q(2*H-2,2*H-2) = -V^2*u_max^2;
Incon(5*H-6).c = xf^2+yf^2;

%1-(xi-cj)^2/aj^2-(yi-dj)^2/bj^2 <= 0
for j =1:m
    for i = 1:H-2
        Incon(5*H-6+(j-1)*(H-2)+i).Q = zeros(2*H-1,2*H-1);
        Incon(5*H-6+(j-1)*(H-2)+i).Q(i,i) = -1/a(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(H-2+i,H-2+i) = -1/b(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(2*H-1,i) = c(j)/a(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(2*H-1,H-2+i) = d(j)/b(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(i,2*H-1) = c(j)/a(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(H-2+i,2*H-1) = d(j)/b(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).c = 1-c(j)^2/a(j)^2-d(j)^2/b(j)^2;
    end
end

% Find number of inequality and equality constraints
nIncon = length(Incon);
nEqcon = length(Eqcon);
n = size(Q0,1);
tic
cvx_solver SeDuMi;
cvx_begin

    % Define size of the unknown matrix
    variable X(n,n);

    % Define cost function
    minimize (trace(X'*Q0));

    % Define equality and inequality quadratic constraints
    subject to
    for j=1:nIncon
        trace(X'*Incon(j).Q) + Incon(j).c <= 0;
    end

    for j=1:nEqcon
        trace(X'*Eqcon(j).Q) + Eqcon(j).c == 0;
    end
    % The unknown matrix 'X' is relaxed as a semidefinite maxtrix
    X == semidefinite(n);
cvx_end

[E, ~] = eig(X);  %E是特征值组成的向量
Vk_ = E(:,1:end-1);  %V0
% k = 1;
wk = w;

cvx_begin
    variable rk nonnegative;
    variable Xk(n,n);
    minimize( trace( Xk'*Q0 ) + wk * rk )
    % Define equality and inequality quadratic constraints
    subject to
    for j=1:nIncon
        trace(Xk'*Incon(j).Q) + Incon(j).c <= 0;
    end

    for j=1:nEqcon
        trace(Xk'*Eqcon(j).Q) + Eqcon(j).c == 0;
    end
    Xk == semidefinite(n);
    rk*eye(n-1) - Vk_'*Xk*Vk_ == semidefinite(n-1);
cvx_end

[Ek, ~] = eig(Xk);  %E是特征值组成的向量
Vk_ = Ek(:,1:end-1); %V1
k = 2;
wk = wk*w;
recordrk = [rk,];   %记录rk的值

% 迭代
while rk > delta

    cvx_begin
        variable rk nonnegative;
        variable Xk(n,n);
        minimize( trace( Xk'*Q0 ) + wk * rk );
        % Define equality and inequality quadratic constraints
        subject to
        for j=1:nIncon
            trace(Xk'*Incon(j).Q) + Incon(j).c <= 0;
        end
        for j=1:nEqcon
            trace(Xk'*Eqcon(j).Q) + Eqcon(j).c == 0;
        end
        Xk == semidefinite(n);
        rk*eye(n-1) - Vk_'*Xk*Vk_ == semidefinite(n-1);
    cvx_end
    fprintf('k=%d,rk = %f,trace(X^{T}*Q0) = %f\n',k,rk,trace( Xk'*Q0 ))
    [Ek, ~] = eig(Xk);
    Vk_ = Ek(:,1:end-1);
    k = k + 1;
    wk = wk*w;
    recordrk = [recordrk,rk];   %记录rk的值
    time_per_iter = [time_per_iter, cvx_cputime];  %记录每次迭代所用时间
end
toc
% Find the solution from IRMA according to
% x=sqrt(\lambda_max)*eigenvactor_max
[EVf,EVa] = eig(Xk);
x = sqrt(max(max(EVa)))*EVf(:,end);

% Output objective value
Jf = trace(Xk'*Q0)

plot([x0;x(1:H-2);xf],[y0;x(H-1:2*H-4);yf],'--o','LineWidth',2)
xlabel('\fontname{Times new roman}\it x')
ylabel('\fontname{Times new roman}\it y')
title(['\fontname{Times new roman}',num2str(H),'\fontname{宋体}个节点',...
    '\fontname{Times new roman}',num2str(m),...
    '\fontname{宋体}个障碍，\fontname{Times new roman}IRM',...
    '\fontname{宋体}法求解的路径'])
axis equal
hold on
theta = 0:0.01:2*pi;
for i = 1:m
    xp = c(i) + a(i) * cos(theta);
    yp = d(i) + b(i) * sin(theta);
    plot(xp, yp, '.r');
end

figure
plot(1:length(recordrk),recordrk,'-o',...
    'Color',[94 213 209]/255,...
    'LineWidth',2,....
    'MarkerSize',6,...
    'MarkerEdgeColor',[219 144 25]/255,...
    'MarkerFaceColor',[255 110 151]/255)
xlabel('\fontname{宋体}迭代次数')
ylabel('\fontname{Times new roman}\it{r_{k}}')
title(['\rm\fontname{Times new roman}\it{r_{k}} \rm\fontname{宋体}随迭代',...
    '次数的变化(\fontname{Times new roman}H',num2str(H),...
    'Obs',num2str(m),')'])

figure
plot(1:length(time_per_iter),time_per_iter,'-go',...
    'LineWidth',2,....
    'MarkerSize',6,...
    'MarkerEdgeColor','m',...
    'MarkerFaceColor','b')
xlabel('\fontname{宋体}迭代次数')
ylabel('\fontname{宋体}时间')
title(['\fontname{宋体}每次迭代所用时间(\fontname{Times new roman}H',...
    num2str(H),'Obs',num2str(m),')'])
